import static com.googlecode.javacv.cpp.opencv_highgui.*;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;
import static com.googlecode.javacv.cpp.opencv_core.CV_AA;
import static com.googlecode.javacv.cpp.opencv_core.CV_RGB;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_EPS;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_ITER;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_32F;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_8U;
import static com.googlecode.javacv.cpp.opencv_core.cvCreateImage;
import static com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_video.cvCalcOpticalFlowPyrLK;

import java.util.ArrayList;
import java.util.List;

import com.googlecode.javacv.FrameGrabber;
import com.googlecode.javacv.FrameGrabber.Exception;
import com.googlecode.javacv.VideoInputFrameGrabber;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.CvSize;
import com.googlecode.javacv.cpp.opencv_core.CvTermCriteria;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_highgui.CvCapture;
import com.googlecode.javacv.cpp.opencv_highgui.CvVideoWriter;

public class VideoProcessor implements Runnable{

    public static double pi = 3.14159265358979323846;
    private static final int MAX_CORNERS = 500;
    static Main delegateMain;
    
    //borders
    private int LEFT; 
    private int RIGHT;
    
    private IplImage frame = null;
    private IplImage frame1 = null;
    private IplImage frame1_1C = null;
    private IplImage frame2_1C = null;
    private IplImage eigImage = null;
    private IplImage tempImage = null;
    private IplImage pyramid1 = null;
    private IplImage pyramid2 = null;
    
    private static final int MAG_THRESH =50;
    private static final int ANGLE_THRESH =150; 
    
    List<Feature> L = new ArrayList<Feature>(); 
    List<Feature> C = new ArrayList<Feature>();
    List<Feature> R = new ArrayList<Feature>();
    
    public double MAX_MAG = 0;
    public double MIN_MAG = 100000;
    
    public double MAX_PH = -Math.PI/2;
    public double MIN_PH = Math.PI/2;
    
    int count;
    double averageMagMaxBuffer = MAG_THRESH-1;
    double averageMagMinBuffer = MAG_THRESH-1;
	double averageMag;
	double averagePhMaxBuffer = ANGLE_THRESH-1;
	double averagePhMinBuffer = ANGLE_THRESH-1;
	double averagePh;
	CvPoint trackedPoint;
	CvVideoWriter out;
	
    //color range of red like color
    static CvScalar min = cvScalar(0, 0, 0, 0);//BGR-A
    static CvScalar max= cvScalar(20,20, 20, 0);//BGR-A
	
	public volatile boolean landing;
	
	static FrameGrabber grabber = null;
	
	public long firstFrame;
    
	public VideoProcessor(Main $delegate){
		delegateMain = $delegate;
		//landing=false;
		landing = true;//testing
	}
	
    public static double square(int a)
    {
        return a * a;
    }
    
    public static IplImage allocateOnDemand(IplImage img, CvSize size, int depth, int channels)
    {
        if(img != null)
        {
            return img;
        }
        else
        {                   
            img = cvCreateImage(size, depth, channels);
            return img;
        }
    }
    
    
    private static void startCamera(){
    	 grabber = new VideoInputFrameGrabber(0);
    	 try {
			grabber.start();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }
    
    private static IplImage captureFrame(){
    	IplImage img = null;

//    	final OpenCVFrameGrabber grabber = new OpenCVFrameGrabber(0);
    	
    	//final VideoCapture grabber = null;
    	/*	try {

				img = grabber.grab();

			} catch (Exception e) {
				// TODO Auto-generated catch block
				
				e.printStackTrace();
				delegateMain.noCamera();
			}
    		
    	//img.getBufferedImage();
    	//img.getBufferedImage();*/
    	
//    	return img;
    	
    	img =  cvQueryFrame(inputVideo);
    	if(img == null){
    		 inputVideo = cvCreateFileCapture("C:/Users/mfelizola/Desktop/docking.avi");
    		 return captureFrame();
    	}
    	return img;
    	
    }
    
    public void setRegions(CvSize size){//not used
    	int y = size.height();
    	int x = size.width();
    	
    	this.LEFT = x/3;
    	this.RIGHT = this.LEFT*2;
    	
    }
    
    static Double abs(double value) {
               
        if(value > 0)
        	return value;
        
        if(value < 0)
        	return -value;
    	
        return (double) 0;
    }
    
    
    
    /**
     * 
     */
    public void findCircle(){
    	
    	  frame1 = captureFrame();
    	  IplImage gray = cvCreateImage(cvGetSize(frame1), 8, 1);
    	   
    	  cvCvtColor(frame1, gray, CV_BGR2GRAY);  
    	  cvSmooth(gray, gray, CV_GAUSSIAN, 1);
    	   
    	  //apply thresholding
          cvInRangeS(frame1, min, max, gray);
    	  //cvThreshold(frame1,max,gray);
          //smooth filter- median
          cvSmooth(gray, gray, CV_MEDIAN, 15);
          //cvSmooth(gray, gray, CV_MEDIAN, 13);
          //cvSmooth(gray, gray, CV_MEDIAN, 13);
          //cvSmooth(gray, gray, CV_MEDIAN, 13);
    	  
    	  CvMemStorage mem = CvMemStorage.create();
    	   
    	  CvSeq circles = cvHoughCircles( 
	    	    gray, //Input image
	    	    mem, //Memory Storage
	    	    CV_HOUGH_GRADIENT, //Detection method
	    	    3, //Inverse ratio
	    	    100, //Minimum distance between the centers of the detected circles
	    	    100, //Higher threshold for canny edge detector
	    	    100, //Threshold at the center detection stage
	    	    1, //min radius
	    	    500 //max radius
    	    );
    	 
    	  CvPoint p = new CvPoint();
          CvPoint q = new CvPoint(); 
          p.x(frame1.width()/2);
          p.y(0);
          
          q.x(frame1.width()/2);
          q.y(frame1.height());
          
          
          CvPoint a = new CvPoint();
          CvPoint b = new CvPoint();  
          a.x(0);
          a.y(frame1.height()/2);
          
          b.x(frame1.width());
          b.y(frame1.height()/2);
          
    	  cvLine(frame1,a,b,CvScalar.BLACK,1,CV_AA,0);
    	  cvLine(frame1,p,q,CvScalar.BLACK,1,CV_AA,0);

    	              //cvLine(frame1, p, q, lineColor, lineThickness, CV_AA, 0);

    	  
    	  for(int i = 0; i < circles.total(); i++){
    	      CvPoint3D32f circle = new CvPoint3D32f(cvGetSeqElem(circles, i));
    	      CvPoint center = cvPointFrom32f(new CvPoint2D32f(circle.x(), circle.y()));
    	      int radius = Math.round(circle.z());      
    	      cvCircle(frame1, center, radius, CvScalar.GREEN, 6, CV_AA, 0);  
    	      cvCircle(frame1, center, 1, CvScalar.GREEN, 6, CV_AA, 0);  

    	      
    	      cvCircle(gray, center, radius, CvScalar.GREEN, 6, CV_AA, 0);
    	      
    	      
    	      delegateMain.dockDetected(round(circle.x()),
    	    		  					round(circle.y()),
    	    		  					radius,
    	    		  					round(frame1.width()),
    	    		  					round(frame1.height()));
    	      
    	      //System.out.println("x = " +circle.x()+ "  y = "+  circle.y() + " in frame with size "+ frame1.width() + " , " + frame1.height());
    	      if(circles.total() > 1){
    	    	  delegateMain.setPane(Messages.MULTIDOCKS);
    	    	  break;
    	      }
    	      
    	      
    	   }
    	  
    	  if(circles.total() < 1){
    		  delegateMain.noDockDetected();
    	  }
    	  
    	  
    	   //frame1 = gray;
    }
    
    
    private int round(double d){
        double dAbs = Math.abs(d);
        int i = (int) dAbs;
        double result = dAbs - (double) i;
        if(result<0.5){
            return d<0 ? -i : i;            
        }else{
            return d<0 ? -(i+1) : i+1;          
        }
    }
    
    public void printImage(IplImage img){
    	    	
        delegateMain.showImage(img.getBufferedImage());
        
    }
    
    public IplImage detect(IplImage img){
    	if( System.currentTimeMillis() - firstFrame > 5000){
    		 CvScalar lineColor = CV_RGB(255,0,0);
    		 int lineThickness = 1;
    		
	    	if(abs(MAX_MAG - averageMag) > this.MAG_THRESH){
	    		
	    		if(averageMagMaxBuffer > MAG_THRESH){
		    		delegateMain.detected(System.currentTimeMillis());
		    		cvCircle(img,trackedPoint,5,lineColor,lineThickness,CV_AA,0);
		    		System.out.println("mag triggered");
		    	}
	    		
	    		averageMagMaxBuffer = abs(MAX_MAG - averageMag);
	    		
	    	}else if(abs(MIN_MAG - averageMag) > this.MAG_THRESH){
	    		
	    		if(averageMagMinBuffer > MAG_THRESH){
		    		delegateMain.detected(System.currentTimeMillis());
		    		cvCircle(img,trackedPoint,5,lineColor,lineThickness,CV_AA,0);
		    		System.out.println("mag triggered min");
	    		}
	    		
	    		averageMagMinBuffer = abs(MIN_MAG - averageMag);
	    		
	    	}else if(abs(MAX_PH - averagePh) > this.ANGLE_THRESH){
	    		
	    		if(averagePhMaxBuffer > ANGLE_THRESH){
		    		delegateMain.detected(System.currentTimeMillis());
		    		cvCircle(img,trackedPoint,5,lineColor,lineThickness,CV_AA,0);
		    		System.out.println("ph triggered max");
	    		}
	    		
	    		averagePhMaxBuffer = abs(MAX_PH - averagePh);
	    		
	    	}else if(abs(MIN_PH - averagePh) > this.ANGLE_THRESH){
	    		
	    		if(averagePhMinBuffer > ANGLE_THRESH){
		    		delegateMain.detected(System.currentTimeMillis());
		    		cvCircle(img,trackedPoint,5,lineColor,lineThickness,CV_AA,0);
		    		System.out.println("ph triggered min");
	    		}
	    		
	    		averagePhMinBuffer =  abs(MIN_PH - averagePh);
	    		
	    	}/**/
	    	System.out.println(" Max mag thresh "+ MAG_THRESH +" Max mag  = " + MAX_MAG + " " + " Average Mag = " + averageMag);
	    	System.out.println(" Max ph thresh "+ ANGLE_THRESH +"Max ph  = " + MAX_PH + " " + " Average ph = " + averagePh);
	      	System.out.println(" ");
	      	
    	}
    	
    	return img;
    }
    
    public void averageMag(double mag, CvPoint p){
    	if(mag > MAX_MAG){
    		MAX_MAG = mag;
    		trackedPoint = p;
    		
    	}
    	
    	if(mag < MIN_MAG){
    		MIN_MAG = mag;
    		trackedPoint = p;
    	}
    	
    	
    	averageMag  = (averageMag + mag) / count;
    }
 
    public void incrementCount(){
    	count++;
    }

    
    public void averagePh(double ph, CvPoint p){
    	if(ph > MAX_PH){
    		MAX_PH = ph;
    		trackedPoint = p;
    	}
    	
    	if(ph < MIN_PH){
    		MIN_PH = ph;
    		trackedPoint = p;
    	}
    	
    	averagePh  = (averagePh + ph) / count;
    }
 

    public void resetAverage(){
    	averageMag = 0;
        averagePh = 0;
        count = 0;

        MAX_MAG = 0;
        MIN_MAG = 100000;
        
        MAX_PH = -Math.PI/2;
        MIN_PH = Math.PI/2;
        
    }
    
    
    public void detectMotion(){
    	
        
        resetAverage();
        
        //int a = cvSetCaptureProperty(inputVideo, CV_CAP_PROP_POS_FRAMES, currentFrame);
        frame = captureFrame();//cvQueryFrame(inputVideo);
        if(frame == null)
        {
            System.out.println("Error:The end come sooner than expected");
            return;
        }   
        CvSize frameSize = new CvSize(frame.width(),frame.height());
        //setRegions(frameSize);
        
        frame1_1C = allocateOnDemand(frame1_1C, frameSize, IPL_DEPTH_8U, 1);
        cvConvertImage(frame, frame1_1C,0);
        
        
        frame1 = allocateOnDemand(frame1, frameSize, IPL_DEPTH_8U, 3);
        cvConvertImage(frame, frame1,0);
        
        //frame = cvQueryFrame(inputVideo);
        frame = captureFrame();
        
        //cvSetCaptureProperty(inputVideo, CV_CAP_PROP_POS_FRAMES, currentFrame+1);
        //frame = cvQueryFrame(inputVideo);
        frame2_1C = allocateOnDemand(frame2_1C, frameSize, IPL_DEPTH_8U, 1);
        cvConvertImage(frame, frame2_1C,0);
       // canvas2.showImage(frame2_1C);
        
        eigImage = allocateOnDemand(eigImage, frameSize, IPL_DEPTH_8U, 1);
        tempImage = allocateOnDemand(tempImage, frameSize, IPL_DEPTH_8U, 1);
        
        CvPoint2D32f frame1Features = new CvPoint2D32f(MAX_CORNERS);
        int[] numberOfFeatures = {MAX_CORNERS};
      
        /*/-----------------------
        int[] corner_count = { max_corners };
        CvPoint2D32f corners = new CvPoint2D32f(max_corners);
        double qltyLevel = 0.05;
        double minDist = 5.0;
        CvArr mask = null;
        int blkSize = 3;
        int useHarris = 1;
        double k = 0.04;
*/
        //cvGoodFeaturesToTrack(gray, eig_img, tmp_img, corners, corner_count, qltyLevel, minDist, mask, blkSize, useHarris, k);
        
        //-----------------------
        cvGoodFeaturesToTrack(frame1_1C, eigImage, tempImage, frame1Features, 
        		numberOfFeatures, .05, 5.0, null,3,0,0.001);
         
        cvFindCornerSubPix(frame1_1C, frame1Features, numberOfFeatures[0],
                cvSize(15, 15), cvSize(5, 5),
                cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
          
          
        CvPoint2D32f frame2Features = new CvPoint2D32f(MAX_CORNERS);
        byte[] opticalFlowFoundFeature = new byte[MAX_CORNERS];
        float[] opticlFlowFeatureError = new float[MAX_CORNERS];
        
        CvSize opticalFlowWindow = cvSize(15,15);
        

        CvTermCriteria opticalFlowTerminationCriteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3 );
        
        CvSize pyr_sz = cvSize(frame1_1C.width() + 8, frame2_1C.height() / 3);
        pyramid1 =  cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
        pyramid2 =  cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
        
        cvCalcOpticalFlowPyrLK(frame1_1C, frame2_1C, pyramid1, pyramid2, frame1Features, frame2Features,
        		numberOfFeatures[0], opticalFlowWindow, 5, 
        		opticalFlowFoundFeature, opticlFlowFeatureError, 
        		opticalFlowTerminationCriteria, 0);
        
        //double [] average = new double[numberOfFeatures[0]];
        double[] averageChange = new double[numberOfFeatures[0]];
        double[] averageAngle = new double[numberOfFeatures[0]];
        for(int i = 0; i<numberOfFeatures[0]; i++)
        {

            if(opticalFlowFoundFeature[i] == 0)
            {
                continue;
            }
            
            int lineThickness = 1;
            CvScalar lineColor = CV_RGB(255,0,0);
            
            CvPoint p = new CvPoint();
            CvPoint q = new CvPoint();  
            p.x((int)frame1Features.position(i).x());
            p.y((int)frame1Features.position(i).y());
            q.x((int)frame2Features.position(i).x());
            q.y((int)frame2Features.position(i).y());
            
            double angle = Math.atan2((double)p.y() - q.y(), (double)p.x() - q.x());
            double hypotenuse = Math.sqrt(square(p.y() - q.y()) + square(p.x() - q.x()));
            averageChange[i] = hypotenuse;//new CvPoint( q.y()-p.y(), q.x()-p.x() );
            averageAngle[i] = angle;
            incrementCount();
            averageMag(hypotenuse,p);
            averagePh(angle,p);
            q.x((int)(p.x() - 10 * hypotenuse * Math.cos(angle)));
            q.y((int)(p.y() - 10 * hypotenuse * Math.sin(angle)));
            //cvCircle(frame1,p,5,lineColor,lineThickness,CV_AA,0);
            //cvLine(frame1, p, q, lineColor, lineThickness, CV_AA, 0);
            
            //p.x((int)(q.x() + 9 * Math.cos(angle + pi / 4)));
            //p.y((int)(q.y() + 9 * Math.sin(angle + pi / 4)));
            
            //cvLine(frame1, p, q, lineColor, lineThickness, CV_AA, 0);
            
            //p.x((int)(q.x() + 9 * Math.cos(angle - pi / 4)));
            //p.y((int)(q.y() + 9 * Math.sin(angle - pi / 4)));
            
            //cvLine(frame1, p, q, lineColor, lineThickness, CV_AA, 0);
            /*if(p.x() < this.LEFT){
            	L.add(new Feature(hypotenuse,angle));
            }else if(p.x() < this.RIGHT){
            	C.add(new Feature(hypotenuse,angle));
            }else{
            	R.add(new Feature(hypotenuse,angle));
            }
            */
            
        }
        frame1 = detect(frame1);
        
        
    }
    
    
    public static CvCapture inputVideo;

	@Override
	public void run() {


        long currentFrame = 0;
        inputVideo = cvCreateFileCapture("C:/Users/mfelizola/Desktop/docking.avi"); //testing

        //startCamera();
        firstFrame = System.currentTimeMillis();
        while(true)
        {
            if(landing){
            	findCircle();
            }else{
            	detectMotion();
            }
            
            delegateMain.showImage(frame1.getBufferedImage());
            
        } 
	}   
}


